#include <Servo.h>
#include <robotLac.h>

robotLac robot(28);
Distance_Sensor dSensor(31, 29);

void setup(){
  robot.begin();
  robot.detener();
  dSensor.begin();
  pinMode(13,OUTPUT); 
  Serial3.begin(9600);
}

void loop (){  
  if(dSensor.get_distance()>0.20){
    robot.avanzar();
    delay(100);
    Serial3.println("AVANZO");
  }
  else{
    robot.detener();
    digitalWrite(13, HIGH);   
    delay(1000);               
    digitalWrite(13, LOW);    
    delay(1000);     
    Serial3.println("ECONTRE PARED");
    
  }
}

